#include "Remote.h"
#include "Solution.h"

RAW_Speed Chassis_Speed={0,0,0,0,0,0,0};
extern RC_t RC;

/**
 * @brief 直接计算遥控器速度到底盘的速度
 * 
 * @param Chassis_Speed 
 * @param rc 
 */
void Calc_Speed(RAW_Speed *Chassis_Speed,RC_t rc){
    uint16_t K=5;//放大系数

    Chassis_Speed->VX=rc.ch0;
    Chassis_Speed->VY=rc.ch1;
    Chassis_Speed->VW=rc.ch2;
    // Get_speed(RC,Chassis_Speed,50,-50,10);
    
    Chassis_Speed->V1=K*(Chassis_Speed->VX * 0.7071068f+Chassis_Speed->VY * 0.7071068f+Chassis_Speed->VW);
    Chassis_Speed->V2=K*(-Chassis_Speed->VX * 0.7071068f+Chassis_Speed->VY * 0.7071068f+Chassis_Speed->VW);
    Chassis_Speed->V3=K*(-Chassis_Speed->VX * 0.7071068f-Chassis_Speed->VY * 0.7071068f+Chassis_Speed->VW);
    Chassis_Speed->V4=K*(Chassis_Speed->VX * 0.7071068f-Chassis_Speed->VY * 0.7071068f+Chassis_Speed->VW);
}

/**
 * @brief 给PID提供速度数据
 * 
 * @param RC 遥控器数据结构体
 * @param max_speed 最大速度
 * @param min_speed 最小速度
 * @param step 步进值
 */
void Get_speed(RC_t RC,RAW_Speed *Chassis_Speed,int16_t max_speed,int16_t min_speed,int16_t step){
    if(RC.ch3 >= 200 && Chassis_Speed->VW < max_speed){
    Chassis_Speed->VW=step*(RC.ch3/1000.0);
    }else if(RC.ch3 <= -200 && Chassis_Speed->VW > min_speed){
    Chassis_Speed->VW+=step*(RC.ch3/1000.0);
    }
}
